1. Field of the Invention
This invention relates generally to a system and method for detecting the state of objects using sensors built into a robot hand and, more particularly, to a system and method for detecting the state of objects that employ particle filters to estimate contact states between a robot hand and an object.
2. Discussion of the Related Art
Autonomous dexterous manipulation has proven to be a challenging area for robotics research. Although many mechanically sophisticated robot hands have been developed, it is difficult to monitor exactly how the manipulator and the object it is grasping make contact. It is appealing to attempt to monitor the hand-object state visually using a camera mounted on a separate pan-tilt platform. However, occlusions that occur during the course of manipulation make localization of the object feature being contacted more difficult. Instead, tactile sensors located near the point of contact of the robot hand may be able to localize the hand-object state more accurately.
Particle filtering is an implementation of a Bayesian state estimation process that estimates system states by integrating a series of sensor measurements over time. In the robotics domain, particle filtering has primarily been applied to the problem of mobile robot localization. There are relatively few approaches to applying particle filtering to dexterous manipulation. In early research, one researcher applied particle filtering to a peg-in-hole insertion task. The peg was mounted on a robot manipulator through a six-axis load cell. A control system moved the peg toward the surface until contact was established. A “contact hyper-surface” was acquired off-line by using the manipulator to create a map of the surface. During localization, the particle filter measurement model matched peg positions to locations on the map. The approach was applied to a forward clutch assembly and localization of a few millimeters was achieved.
In related work, another researcher applied particle filtering to the problem of localizing a modeled inelastic object. An end-effector was mounted on a manipulator through a six-axis load cell. The load cell moved toward the object until contact was established. Position measurements were matched to an analytic model and localization was achieved. A method was proposed by the researcher for extending the technique to an arbitrary triangulated object.
Yet another researcher took a similar approach to localizing the position of a single contact on a known surface. As in the above work, localization was performed by measuring contact position while the manipulator touched the surface of the object. This work used an active exploration strategy to move the contact so as to achieve localization more quickly.
Particle filtering models the system as a hidden Markov process where the system state is hidden. Sensor measurements (“observations”) may partially reflect the state. On each time step, the system begins in state yt−1, executes a control signal ut, and arrives at state yt while making an observation zt. Because the process is a Markov, transitions to future states are conditionally independent of all previous observations Zt=z1 . . . zt and control signals Ut=u1 . . . ut:P(yt|Zt,Ut)  (1)This distribution is called the “belief state.”
Starting with an arbitrary (possibly uniform) prior distribution, particle filtering iteratively updates the belief state on each time step by incorporating the last control signal and measurement. This is accomplished in two steps. The first step (the motion update) incorporates the most recent control signal into the belief state:
                                                                        P                ⁡                                  (                                                                                    y                        t                                            |                                              Z                                                  t                          -                          1                                                                                      ,                                          U                      t                                                        )                                            =                            ⁢                                                ∑                                      y                                          t                      -                      1                                                                      ⁢                                                      P                    ⁡                                          (                                                                                                    y                            t                                                    |                                                      y                                                          t                              -                              1                                                                                                      ,                                                  Z                                                      t                            -                            1                                                                          ,                                                  U                          t                                                                    )                                                        ⁢                                      P                    ⁡                                          (                                                                                                    y                                                          t                              -                              1                                                                                |                                                      Z                                                          t                              -                              1                                                                                                      ,                                                  U                          t                                                                    )                                                                                                                                              =                            ⁢                                                ∑                                      y                                          t                      -                      1                                                                      ⁢                                                      P                    ⁡                                          (                                                                                                    y                            t                                                    |                                                      y                                                          t                              -                              1                                                                                                      ,                                                  u                          t                                                                    )                                                        ⁢                                      P                    ⁡                                          (                                                                                                    y                                                          t                              -                              1                                                                                |                                                      Z                                                          t                              -                              1                                                                                                      ,                                                  U                                                      t                            -                            1                                                                                              )                                                                                                                              (        2        )            where the Markov assumption enables the simplification P(yt|yt−1, ut)=P(yt−1|Zt−1, Ut) and it is possible to conclude that P(yt−1|Zt−1, Ut−1)=P(yt−1|Zt−1, Ut) because current state is independent of future contact signals.
The second step (the measurement update) incorporates the most recent measurement as:
                                                                        P                ⁡                                  (                                                                                    y                        t                                            |                                              Z                        t                                                              ,                                          αΔ                      ⁢                                                                                          ⁢                                              U                        t                                                                              )                                            =                            ⁢                                                                    P                    ⁡                                          (                                                                                                    z                            t                                                    |                                                      y                            t                                                                          ,                                                  Z                                                      t                            -                            1                                                                          ,                                                  U                          t                                                                    )                                                        ⁢                                      P                    ⁡                                          (                                                                                                    y                            t                                                    |                                                      Z                                                          t                              -                              1                                                                                                      ,                                                  U                          t                                                                    )                                                                                        P                  ⁡                                      (                                                                                            z                          t                                                |                                                  Z                                                      t                            -                            1                                                                                              ,                                              U                        t                                                              )                                                                                                                          =                            ⁢                              η                ⁢                                                                  ⁢                                  P                  ⁡                                      (                                                                  z                        t                                            |                                              y                        t                                                              )                                                  ⁢                                  P                  ⁡                                      (                                                                                            y                          t                                                |                                                  Z                                                      t                            -                            1                                                                                              ,                                              U                        t                                                              )                                                                                                          (        3        )            Where η=P(zt|Zt−1, Ut) is assumed to be constant. The Markov assumptions enable the equality P(zt|yt)=P(Zt|yt, Zt−1, Ut).
Rather than storing the belief state distribution explicitly, the particle filter maintains a finite set of k points St=st1 . . . stk such that the probability that a state belongs to this set is proportional to P(yt|Zt, Ut). In the context of the particle filter, the motion and measurement update steps described above become the following steps.
1. for all stjε St 
2. stj,=MotionUpdate (stj, ut)
3. wj=ηP(zt|st′j)
4. Resample St+1 from St, according to the weights W=w1 . . . wk 
In step 2, each particle is stochastically displaced according to a MotionUpdate function that implicitly encodes P(yt|yt−1, ut). In step 3, each of the particles is weighted according to how likely the observation is from the new state. Finally, step 4 creates an updated set of k particles St+1 by sampling from St in proportion to the weights W.